/* Includes ------------------------------------------------------------------*/
#include <stdarg.h>
#include <stdio.h>
#include "stm8s.h"
#include "utils.h"
#include "board_conf.h"

uint32_t opt_to_baudrate(uint16_t baudrate_opt)
{
    uint32_t baudrate;
    
    switch (baudrate_opt) {
    case BR_2400: baudrate = 2400; break;
    case BR_4800: baudrate = 4800; break;
    case BR_9600: baudrate = 9600; break;
    case BR_19200: baudrate = 19200; break;
    case BR_38400: baudrate = 38400; break;
    default : baudrate = 9600; break;
    }
    
    return baudrate;
}

uint8_t read_parameters(void * par_buf)
{
    uint8_t i;
    uint32_t addr = FLASH_DATA_START_PHYSICAL_ADDRESS;
    uint8_t check_bytes[4] = PAR_CHECK_BYTES;
    
    for (i = 0; i < 4; i++) {
        if (check_bytes[i] != FLASH_ReadByte(addr++)) {
            /* Check Failed! */
            return 1;
        }
    }

    for (i = 0; i < sizeof(device_parameters_t); i++) {
        *((uint8_t *)par_buf + i) = FLASH_ReadByte(addr++);
    }
    
    return 0;
}

void save_parameters(void * par_buf, uint8_t par_selection)
{
    uint8_t i;
    uint32_t addr = FLASH_DATA_START_PHYSICAL_ADDRESS;
    uint8_t check_bytes[4] = PAR_CHECK_BYTES;
    
    /* Unlock flash data eeprom memory */
    FLASH_Unlock(FLASH_MEMTYPE_DATA);
    /* Wait until Data EEPROM area unlocked flag is set*/
    while (FLASH_GetFlagStatus(FLASH_FLAG_DUL) == RESET)
    {}
    
    /* Clear check bytes*/
    for (i = 0; i < 4; i++) {
        FLASH_ProgramByte(addr++, 0x00);
    }
    
    /* Write parameters */
    addr = FLASH_DATA_START_PHYSICAL_ADDRESS + 4;
    switch (par_selection) {
    case PAR_BAUDRATE_OPT:
        FLASH_ProgramByte(addr++ + PAR_BAUDRATE_OPT_OFFSET, ((uint8_t *)par_buf)[PAR_BAUDRATE_OPT_OFFSET]);
        FLASH_ProgramByte(addr++ + PAR_BAUDRATE_OPT_OFFSET, ((uint8_t *)par_buf)[PAR_BAUDRATE_OPT_OFFSET+1]);
        break;
    case PAR_OFFSET_ADDR:
        FLASH_ProgramByte(addr++ + PAR_OFFSET_ADDR_OFFSET, ((uint8_t *)par_buf)[PAR_OFFSET_ADDR_OFFSET]);
        FLASH_ProgramByte(addr++ + PAR_OFFSET_ADDR_OFFSET, ((uint8_t *)par_buf)[PAR_OFFSET_ADDR_OFFSET+1]);
        break;
    case PAR_OP_MODE:
        FLASH_ProgramByte(addr++ + PAR_OP_MODE_OFFSET, ((uint8_t *)par_buf)[PAR_OP_MODE_OFFSET]);
        FLASH_ProgramByte(addr++ + PAR_OP_MODE_OFFSET, ((uint8_t *)par_buf)[PAR_OP_MODE_OFFSET+1]);
        break;
    case PAR_DELAY_TIME:
        FLASH_ProgramByte(addr++ + PAR_DELAY_TIME_OFFSET, ((uint8_t *)par_buf)[PAR_DELAY_TIME_OFFSET]);
        FLASH_ProgramByte(addr++ + PAR_DELAY_TIME_OFFSET, ((uint8_t *)par_buf)[PAR_DELAY_TIME_OFFSET+1]);
        break;
    case PAR_ALL:
        for (i = 0; i < sizeof(device_parameters_t); i++) {
            FLASH_ProgramByte(addr++, ((uint8_t *)par_buf)[i]);
        }
        break;
    default: break;
    }
    /* Write check bytes*/
    addr = FLASH_DATA_START_PHYSICAL_ADDRESS;
    for (i = 0; i < 4; i++) {
        FLASH_ProgramByte(addr++, check_bytes[i]);
    }
    
    /* Lock flash data eeprom memory */
    FLASH_Lock(FLASH_MEMTYPE_DATA);
}

uint8_t read_config_address(void)
{
    uint8_t tmp, r = 0;
    
    tmp = GPIO_ReadOutputData(GPIOB);
    
    if (tmp & (1 << 7)) {
        r |= 1 << 0;
    }
    if (tmp & (1 << 6)) {
        r |= 1 << 1;
    }
    if (tmp & (1 << 3)) {
        r |= 1 << 2;
    }
    if (tmp & (1 << 2)) {
        r |= 1 << 3;
    }
    
    return r;
}

void read_coils(uint8_t *coil_buf)
{
    uint8_t tmpc, tmpd;
    
    tmpc = GPIO_ReadOutputData(GPIOC);
    tmpd = GPIO_ReadOutputData(GPIOD);
    
    coil_buf[0] = 0;
    
    if (tmpc & (1 << 5)) {
        coil_buf[0] |= 1 << 0;
    }
    if (tmpc & (1 << 4)) {
        coil_buf[0] |= 1 << 1;
    }
    if (tmpc & (1 << 3)) {
        coil_buf[0] |= 1 << 2;
    }
    if (tmpc & (1 << 6)) {
        coil_buf[0] |= 1 << 3;
    }
    if (tmpd & (1 << 2)) {
        coil_buf[0] |= 1 << 4;
    }
    if (tmpd & (1 << 0)) {
        coil_buf[0] |= 1 << 5;
    }
    if (tmpc & (1 << 7)) {
        coil_buf[0] |= 1 << 6;
    }
    if (tmpc & (1 << 1)) {
        coil_buf[0] |= 1 << 7;
    }
}

void set_coils(uint8_t *coil_buf)
{
    uint8_t tmpc = 0, tmpd = 0;
    
    if (coil_buf[0] & (1 << 0)) {
        tmpc |= 1 << 5;
    }
    if (coil_buf[0] & (1 << 1)) {
        tmpc |= 1 << 4;
    }
    if (coil_buf[0] & (1 << 2)) {
        tmpc |= 1 << 3;
    }
    if (coil_buf[0] & (1 << 3)) {
        tmpc |= 1 << 6;
    }
    if (coil_buf[0] & (1 << 4)) {
        tmpd |= 1 << 2;
    }
    if (coil_buf[0] & (1 << 5)) {
        tmpd |= 1 << 0;
    }
    if (coil_buf[0] & (1 << 6)) {
        tmpc |= 1 << 7;
    }
    if (coil_buf[0] & (1 << 7)) {
        tmpc |= 1 << 1;
    }
    
    GPIO_Write(GPIOC, tmpc);
    GPIO_Write(GPIOD, tmpd);
}

void read_discrete_inputs(uint8_t *discrete_input_buf)
{
    uint8_t tmpa, tmpb, tmpf;
    
    tmpa = GPIO_ReadInputData(GPIOA);
    tmpb = GPIO_ReadInputData(GPIOB);
    tmpf = GPIO_ReadInputData(GPIOF);
    
    discrete_input_buf[0] = 0;
    if (tmpa & (1 << 1)) {
        discrete_input_buf[0] |= 1 << 0;
    }
    if (tmpa & (1 << 2)) {
        discrete_input_buf[0] |= 1 << 1;
    }
    if (tmpa & (1 << 3)) {
        discrete_input_buf[0] |= 1 << 2;
    }
    if (tmpf & (1 << 4)) {
        discrete_input_buf[0] |= 1 << 3;
    }
    if (tmpb & (1 << 7)) {
        discrete_input_buf[0] |= 1 << 4;
    }
    if (tmpb & (1 << 6)) {
        discrete_input_buf[0] |= 1 << 5;
    }
    if (tmpb & (1 << 3)) {
        discrete_input_buf[0] |= 1 << 6;
    }
    if (tmpb & (1 << 2)) {
        discrete_input_buf[0] |= 1 << 7;
    }
}

void led_ctrl(void)
{
    static uint8_t led_cnt = 0;
    
    if (led_cnt == 2) {
        LED_Off(LED0);
    } 
    else if (led_cnt == 30) {
        LED_On(LED0);
        led_cnt = 0;
    }
    else {
    }
    
    led_cnt++;
}
